% e-enmpc版本三，加入环境温度异常扰动（类似隧道岩温升高）
clear;
close all;
dbstop if error
%% 添加casadi
addpath("E:\Coding\matlab\casadi-3.6.7-windows64-matlab2018b");
addpath("base_func\");
import casadi.*

%% 处理线路数据
% 线路1
% CurveData = readmatrix("real_line\北京北-崇礼\曲线文件输入-递增.xls");
% GradientData = readmatrix("virtual_line\line_info.xlsx","Sheet","坡度2");
% SpeedLimitData = readmatrix("virtual_line\line_info.xlsx","Sheet","限速3");

% 线路2
GradientData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","坡道");
CurveData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","曲线");
CurveData = [CurveData(:,1:2).*1000 CurveData(:,4)];
SpeedLimitData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","限速");
TunnelData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","隧道");

%处理成仅3列，且把 km->m
GradientData = [GradientData(:,1:2).*1000 GradientData(:,3)];
CurveData = [CurveData(:,1:2).*1000 CurveData(:,3)];
SpeedLimitData = [SpeedLimitData(:,1:2).*1000 SpeedLimitData(:,3)];
TunnelData(:,4) = []; %原隧道数据
%曲线数据补充平直道
CurveData = fill_curvedata(CurveData);
tunnelData = fill_curvedata(TunnelData); %填充无隧道段的原理跟填充曲线一样，函数复用
%补充缺失的限速数据
SpeedLimitData = fill_speedlimit(SpeedLimitData,200);

%% 结构体1：线路参数
LineInfo.gradient = GradientData;
LineInfo.curve = CurveData;
LineInfo.speedlimit = SpeedLimitData;
LineInfo.tunnel = tunnelData;
LineInfo.init_height = 640; %初始海拔

%% 结构体2：列车参数
TrainInfo.abc = [0.55 0.003622 0.00011]; %CRH380B
TrainInfo.mass = 425; %吨 CRH380B - AW1
TrainInfo.rf = 0.06; % rotate factor 回转质量系数
TrainInfo.eta_gear = 0.9; %齿轮箱输出和输入功率之比
TrainInfo.motor_num = 16;

%% 结构体3：求解参数
QsInfo.pos_start = 150;   %起点
QsInfo.pos_end = 300; %终点
QsInfo.v_start = 0; % km/h
QsInfo.v_end = 0;
QsInfo.set_time = 3500; %计划剩余时间
QsInfo.t_now = 0; %起始时刻
QsInfo.step_x = 20; %求解步长1
QsInfo.step_t = 5; 
QsInfo.t_err = 5; %达时运行浮动时间
QsInfo.T_st = [60 80]; %电机起始温度
QsInfo.T_env_base = 30;

%% 隧道岩温异常设置
n_tun = length(TunnelData);
for i=1:n_tun
    if TunnelData(i,1) > QsInfo.pos_start
        ns_tun = i;
        break;
    end
end
for i=1:n_tun
    if TunnelData(i,1) > QsInfo.pos_end
        ne_tun = i-1;
        break;
    end
end
fprintf("运行区间中有隧道：%d 号至 %d 号\n",ns_tun,ne_tun);
num_fault_tun = 35; %假设第35号隧道温度异常
T_fault_tun = 70; %岩温升高后的温度

%% 计算
%计算最大能力曲线
% state = speedPlanMP(LineInfo, TrainInfo, QsInfo);
% state = speedPlanMP(LineInfo, TrainInfo, QsInfo);
% t_mp_min = state(end,3);
%计算恒速曲线
state = speedplan_cruise(LineInfo, TrainInfo, QsInfo);
% xvtfhg = [state(:,1:3) state(:,4)-state(:,5) state(:,13) state(:,11)];
tvxfhg_temp = convert_pos_to_time_discrete(state, QsInfo);
%模拟列车走行时间
t_end = QsInfo.set_time + 10; %计算终止时间富余10s
dt = 0.2; %牵引计算时间步长
tcr_ = [0:dt:t_end]';
%牵引计算恒速优化
cr_T(1,:) = QsInfo.T_st;
cr_v_kmh = zeros(length(tcr_),1);
% tcr = zeros(length(tcr_),1);
cr_f = zeros(length(tcr_),1);
cr_fe = zeros(length(tcr_),1);
cr_s = zeros(length(tcr_),1);
cr_s(1) = QsInfo.pos_start*1000; %初始位置
cr_ploss = zeros(length(tcr_),4);
mode = 0;
%位置离散牵引计算
for i = 1:length(tcr_)-1
    % 牵/制力
    if cr_s(i) > state(end,1) %超过终点赋值为终值
        cr_f(i) = state(end-1,4) - state(end-1,5);
    else
        for ist = 1:length(state)-1
            if cr_s(i) >= state(ist,1) && cr_s(i) <= state(ist+1,1)
                cr_f(i) = state(ist,4) - state(ist,5);
                %计算电制动力值
                if(state(ist,5) > 0)
                   cr_fe(i) = min([state(ist,5) getMaxFeb(cr_v_kmh(i))]);
                end
                break;
            end
        end
    end
    
    % 计算基本运行阻力
    f_rb = cal_unit_basicResist(TrainInfo.abc,cr_v_kmh(i)) * TrainInfo.mass * 9.81 / 1000;
    % 计算线路附加阻力
    f_rl = cal_unit_lineResist(cr_s(i),LineInfo.gradient,LineInfo.curve) * TrainInfo.mass * 9.81 / 1000;
    % 计算加速度
    a = (cr_f(i)-f_rb-f_rl)/TrainInfo.mass/(1+TrainInfo.rf);
    % 计算下一状态速度
    cr_v_kmh(i+1) = cr_v_kmh(i) + a*dt*3.6;
    if cr_v_kmh(i+1) < 0
        cr_v_kmh(i+1) = 0;
        cr_s(i+1) = cr_s(i);
    else
        % 计算下一状态位置
        cr_s(i+1) = cr_s(i) + cr_v_kmh(i)/3.6*dt + 1/2*a*dt^2;
    end
    %计算平均分配下每个电机的力
    if cr_f(i) < -getMaxFeb(cr_v_kmh(i))
        Fpmt = -getMaxFeb(cr_v_kmh(i))/TrainInfo.motor_num;
    else
        Fpmt = cr_f(i)/TrainInfo.motor_num;
    end
    %获取环境温度
    h = getPosHeight(cr_s(i),LineInfo); %当前位置海拔
    envTheta = QsInfo.T_env_base - 6*h/1000;
    if cr_s(i) > TunnelData(num_fault_tun,1)*1000 && cr_s(i) < TunnelData(num_fault_tun,2)*1000
        envTheta = T_fault_tun; %列车运行到异常温度区间
    end
    %计算电机温度
    v_kmh = (cr_v_kmh(i)+cr_v_kmh(i+1))/2;
    [cr_ploss(i,1),cr_ploss(i,2),cr_ploss(i,3),cr_ploss(i,4)] = getmotorlossv2(Fpmt, v_kmh);
    dt_ = tcr_(i+1)-tcr_(i);
    h = getPosHeight(cr_s(i),LineInfo);
    [cr_T(i+1,1), cr_T(i+1,2)] = lptn2nodes_sr5(cr_ploss(i,1:2),cr_T(i,1:2),h,dt_,envTheta);
end
%计算停车时刻
for ie = length(tcr_):-1:1
    if cr_v_kmh(ie) ~= 0
        tcr_stop = tcr_(ie);
        break;
    end
end
%% 初始优化
%权重 速度 | 位置 | 温度 | 电牵制力代价 | 空气制动代价 | 终端代价
w1 = 1e-1;
w2 = 1e-2;
w3 = 50;
w4 = 1e-2;
w5 = 400;
w6 = 1e6;
weights = [w1 w2 w3 w4 w5 w6];
[res, casadi_flag] = atc_opt_v4(tvxfhg_temp, weights, LineInfo, TrainInfo, QsInfo, []);
last_res = res; %对于下一次优化而言存储上一次优化结果
% res = [td_' res];
%% 创建动态图窗
figure(3)
set(gcf,'Position',[100,150,1000,400]);
yyaxis right;
hl1 = area(state(:,1)./1000,state(:,13),'FaceColor',[0.7 0.7 0.7],'EdgeColor', [0.6 0.6 0.6],"FaceAlpha",0.35);
set(hl1, 'DisplayName', '坡度');
ylim([0 5000]);
xlim([QsInfo.pos_start-10 QsInfo.pos_end+10]);
yyaxis left;
h1 = plot(res(:,3)./1000,res(:,2).*3.6,'LineWidth',1.5);
set(h1, 'DisplayName', "原始优化结果");
ylim([0 230]);
hold on;
xlabel('位置km');
ylabel('速度km/h');
% draw_res(res,3);
%% 执行nmpc过程
tst_nmpc = tic;
%牵引计算时间
% t_end = QsInfo.set_time + QsInfo.t_err; %计算终止时间
t_end = QsInfo.set_time + 20; %计算终止时间富余10s
qs_info = QsInfo; %用于事件触发重优化的结构体
dt = 0.2; %牵引计算时间步长
tc = [0:dt:t_end]';
%过程参数初始化
mpc_s = zeros(length(tc),1);
mpc_f = zeros(length(tc),1);
mpc_fe = zeros(length(tc),1);
mpc_fm = zeros(length(tc),1);
mpc_T_env = zeros(length(tc),1);
mpc_T_env(1) = QsInfo.T_env_base - 6*getPosHeight(QsInfo.pos_start*1000,LineInfo)/1000;
mpc_s(1) = QsInfo.pos_start*1000;
mpc_vms = zeros(length(tc),1); % m/s
mpc_ploss = zeros(length(tc),4);
mpc_T = zeros(length(tc),2);
mpc_T(1,:) = QsInfo.T_st;
last_mod = 0;
flag_opt = 0; %事件触发标志
pc_count = 1;
opt_proc = {};
tk = 0; %记录重规划次数
for i=1:length(tc)
    t = tc(i);
    for j = 1:length(res)-1
        if t >= res(j,1) && t <= res(j+1,1) || abs(t-res(j,1)) < 1e-6 %寻找优化结果中的对应序列
            v_p = res(j,2); %预测的速度
            s_p = res(j,3); %预测的位置 
            Ts_p = res(j,4); %预测的定子温度
            Tr_p = res(j,5); %预测的转子温度
            mpc_fe(i) = res(j,6); %电机发挥力
            mpc_fm(i) = res(j,7); %空气制动力
            mpc_f(i) = mpc_fe(i) - mpc_fm(i);
            Fpmt = mpc_fe(i) / TrainInfo.motor_num; %单个电机的发挥力
            break;
        end
        if t > QsInfo.set_time  %超过预设时间，应该制动了
            v_p = 0; %预测的速度
            s_p = qs_info.pos_end; %预测的位置 
            Ts_p = res(j,4); %预测的定子温度
            Tr_p = res(j,5); %预测的转子温度
            mpc_fe(i) = -getMaxFeb(mpc_vms(i)*3.6); %电机发挥力
            mpc_fm(i) = 200; %空气制动力
            mpc_f(i) = mpc_fe(i) - mpc_fm(i);
            Fpmt = mpc_fe(i) / TrainInfo.motor_num; %单个电机的发挥力
            break;
        end
    end
    % 牵引计算
    % 计算基本运行阻力
    f_rb = cal_unit_basicResist(TrainInfo.abc,mpc_vms(i)*3.6) * TrainInfo.mass * 9.81 / 1000;
    % 计算线路附加阻力
    f_rl = cal_unit_lineResist(mpc_s(i),LineInfo.gradient,LineInfo.curve) * TrainInfo.mass * 9.81 / 1000;
    % 计算加速度
    a = (mpc_f(i)-f_rb-f_rl)/TrainInfo.mass/(1+TrainInfo.rf);
    % 计算下一状态速度
    mpc_vms(i+1) = mpc_vms(i) + a*dt;
    % 判断制动停车
    if mpc_vms(i+1) < 0
        mpc_vms(i+1) = 0;
        if mpc_vms(i) > 0
            mpc_s(i+1) = mpc_s(i) + mpc_vms(i)*dt + 1/2*a*dt^2;
        else
            mpc_s(i+1) = mpc_s(i);
        end
    else
        % 计算下一状态位置
        mpc_s(i+1) = mpc_s(i) + mpc_vms(i)*dt + 1/2*a*dt^2;
    end
    % 计算下一状态时间
    tn = t + dt;
    % 计算电机损耗
    [mpc_ploss(i,1),mpc_ploss(i,2),mpc_ploss(i,3),mpc_ploss(i,4)] = getmotorlossv2(Fpmt, mpc_vms(i+1)*3.6);
    % 计算当前环境温度
    h = getPosHeight(mpc_s(i+1),LineInfo);
    envTheta = QsInfo.T_env_base - 6*h/1000; %一般情况下环境温度考虑为1000m下降6度
    if mpc_s(i) > TunnelData(num_fault_tun,1)*1000 && mpc_s(i) < TunnelData(num_fault_tun,2)*1000
        envTheta = T_fault_tun; %列车运行到异常温度区间
    end
    mpc_T_env(i+1) = envTheta;
    % 解算电机实际温度
    [mpc_T(i+1,1), mpc_T(i+1,2)] = lptn2nodes_sr5(mpc_ploss(i,1:2),mpc_T(i,1:2),h,dt,envTheta);
    
    % 事件触发重规划
    switch flag_opt
        case 0 %状态0：判断事件触发重规划
            t_s0 = t; %记录状态0时间戳
            % 事件触发重规划：周期重规划
            % tk = floor(t/240);
            % if abs(res(j,3)-mpc_s(i+1)) > 50 && tk > last_mod && QsInfo.set_time-t >= 120
            %     flag_opt = 1;
            %     last_mod = tk;
            % end
            % 速度偏离事件
            if abs(mpc_vms(i)-v_p) > 1.1
                flag_opt = 1;
            end
            % 温度偏离事件
            if mpc_T(i,1)-Ts_p > 10 || mpc_T(i,2)-Tr_p > 10
                flag_opt = 1;
            end
            % 剩余时间过少事件
            if QsInfo.set_time-t < 60
                flag_opt = 0;
            end
        case 1 %重规划
            tst_plan = tic;
            tk = tk+1;
            %重规划更新求解配置
            qs_info.pos_start = mpc_s(i+1)./1000;
            qs_info.v_start = mpc_vms(i+1)*3.6;
            qs_info.t_now = tn; %更新当前时间
            qs_info.set_time = QsInfo.set_time - qs_info.t_now;  %更新剩余时间
            qs_info.T_st = [mpc_T(i+1,1), mpc_T(i+1,2)];
            %根据运行时间调整权重
            w2 = 1e-2 + (1-1e-2)*t/QsInfo.set_time;
            weights = [w1 w2 w3 w4 w5 w6];
            %调整时间步长
            if qs_info.set_time/qs_info.step_t < 200 && qs_info.step_t > 1
                qs_info.step_t = qs_info.step_t - 1;
            end
            
            %更新恒速曲线
            t_cruise_st = tic;
            state_temp = speedplan_cruise(LineInfo, TrainInfo, qs_info);
            tvxfhg_temp = convert_pos_to_time_discrete(state_temp,qs_info);
            fprintf("-函数恒速规划耗时：%f s\n",toc(t_cruise_st));
            %跟据恒速曲线计算线路各位置的环境温度
            theta_env_vec = QsInfo.T_env_base*ones(length(tvxfhg_temp),1) - 6.*tvxfhg_temp(:,5)./1000;
            if mpc_s(i) > TunnelData(num_fault_tun,1)*1000 && mpc_s(i) < TunnelData(num_fault_tun,2)*1000 %如果列车运行到异常温度区间，更新未来预估环境温度
                for it1 = 1:length(tvxfhg_temp)
                    if tvxfhg_temp(it1,3) < TunnelData(num_fault_tun,2)*1000  %如果有岩温异常默认未来整条隧道都保持这种温度
                        theta_env_vec(it1) = envTheta;
                    end
                end
            end
            tvxfhg_temp = [tvxfhg_temp theta_env_vec];

            %优化求解
            t_optf_st = tic;
            [res, casadi_flag] = atc_opt_v5(tvxfhg_temp, weights, LineInfo, TrainInfo, qs_info, last_res);
            fprintf("-函数atc_opt耗时：%f s\n",toc(t_optf_st));
            opt_proc{1,tk} = res;
            %判断是否优化成功
            if ~casadi_flag
                for io = tk:-1:1
                    if ~isempty(opt_proc{1,io})
                        res = opt_proc{1,io};
                        last_res = res;
                        break;
                    end
                end
            end
            % res = [td_' res];
            % h1 = plot(res(:,3)./1000,res(:,2).*3.6);
            % set(h1, 'DisplayName', "第"+num2str(tk)+"次");
            plot(qs_info.pos_start,qs_info.v_start,'Marker','|','Color','g','LineWidth',3,'HandleVisibility', 'off'); %画事件触发点
            if mpc_s(i) > TunnelData(num_fault_tun,1)*1000 && mpc_s(i) < TunnelData(num_fault_tun,2)*1000
                h1 = plot(res(:,3)./1000,res(:,2).*3.6, 'LineStyle','--','Marker','none','Color',rand(1,3));
                set(h1, 'DisplayName', "第"+num2str(tk)+"次");
            end
            % h2 = plot(mpc_s(pc_count:i+1)./1000,mpc_vms(pc_count:i+1)*3.6,'LineWidth',2,'LineStyle',':','Color',[0.6350 0.0780 0.1840],'HandleVisibility', 'off');
            % pc_count = i+2;
            lgd = legend('show','Location','south');
            % draw_res(res,4);
            flag_opt = 2; %跳到状态2
            ted_plan = toc(tst_plan);
            fprintf("--第"+num2str(tk)+"次滚动优化"+"耗时：%f s\n",ted_plan);
        case 2 %重规划最小间隔
            if t - t_s0 > 30 
                flag_opt = 0;
            end
    end
end
%衔接上面的figure3
h2 = plot(mpc_s./1000,mpc_vms*3.6,'LineWidth',2,'LineStyle',':','Color',[0.6350 0.0780 0.1840],'HandleVisibility', 'on', 'Marker','none');
set(h2, 'DisplayName', "牵引计算");
ylim([0 250]);
hold off;
ted_nmpc = toc(tst_nmpc);
fprintf("总计用时：%f s\n",ted_nmpc);

%% 效果统计
T_stator = real(mpc_T(:,1));
T_rotor = real(mpc_T(:,2));
%位置误差统计
x_end_err = qs_info.pos_end*1000 - mpc_s(end);
fprintf("终点误差：%f m\n",x_end_err);
% 查找停车时刻
i = length(mpc_vms);
t_stop = tc(i-1);
while mpc_vms(i) == 0
    t_stop = tc(i-1);
    i = i-1;
end
t_end_err = t_stop - QsInfo.set_time;
fprintf("时间误差：%f s\n",t_end_err);
%能耗统计：优化
recycle_ratio = 0.5;
energy_opt = 0;
energy_opt_brake = 0;
for i=1:length(mpc_fe)
    dx_ = mpc_s(i+1) - mpc_s(i);
    energy_opt = energy_opt + mpc_fe(i)*dx_*(mpc_fe(i)>0); %纯牵引能耗
end
energy_opt_kwh = energy_opt / 3600;
%能耗统计：恒速
energy_cruise = state(1:end-1,4)' * (state(2:end,1)-state(1:end-1,1)); %纯牵引能耗
energy_cruise_brake = state(1:end-1,5)' * (state(2:end,1)-state(1:end-1,1)); %电制动能耗
energy_cruise_kwh = energy_cruise / 3600;
fprintf("轮周牵引能耗—— 恒速：%f kWh | nmpc：%f kWh\n",energy_cruise_kwh,energy_opt_kwh);
%能耗统计：耗散
motor_loss_cruise_kwh = TrainInfo.motor_num .* real(cr_ploss(1:end-1,3))' * real(tcr_(2:end)-tcr_(1:end-1))  ./ 3600; %恒速耗散
motor_loss_nmpc_kmh = TrainInfo.motor_num .* real(mpc_ploss(1:end-1,3))' * real(tc(2:end)-tc(1:end-1))  ./ 3600;
%牵引节能率
ee_ratio = (energy_cruise_kwh-energy_opt_kwh)/energy_cruise_kwh * 100;
fprintf("牵引能耗节能率：%f\n",ee_ratio);
%电机输入功率侧节能率
pmotor_in_cruise = energy_cruise_kwh+motor_loss_cruise_kwh;
pmotor_in_nmpc = energy_opt_kwh+motor_loss_nmpc_kmh;
ee_ratio2 = (pmotor_in_cruise - pmotor_in_nmpc)/pmotor_in_cruise * 100;
fprintf("电机侧输入能耗—— 恒速：%f kWh | nmpc：%f kWh\n",pmotor_in_cruise,pmotor_in_nmpc);
fprintf("电机输入能耗节能率：%f\n",ee_ratio2);
%再生制动总有效功率 回馈制动能量减去发热耗散的能量
%恒速优化下的再生制动总能耗
ec1_vec = cr_fe(1:end-1) .* (cr_s(2:end)-cr_s(1:end-1)) - ...
        TrainInfo.motor_num .*cr_ploss(1:end-1,3).*(tcr_(2:end)-tcr_(1:end-1)).*(cr_fe(1:end-1)>0);
ec1 = sum(ec1_vec)./3600;
%nmpc优化下的再生制动总能耗
eo1_vec = -mpc_fe(1:end-1).*(mpc_s(2:end-1) - mpc_s(1:end-2)).*(mpc_fe(1:end-1)<0) - ...
    TrainInfo.motor_num .*mpc_ploss(1:end-1,3).*(tc(2:end)-tc(1:end-1)).*(mpc_fe(1:end-1)<0);
eo1 = sum(eo1_vec)./3600;
fprintf("电机侧再生制动能量—— 恒速：%f kWh | nmpc：%f kWh\n",ec1,eo1);
%考虑再生制动节能率
energy_cruise_plus_eb = pmotor_in_cruise-ec1*recycle_ratio;
energy_opt_plus_eb = pmotor_in_nmpc-eo1*recycle_ratio;
ee_ratio3 = (energy_cruise_plus_eb-energy_opt_plus_eb) / energy_cruise_plus_eb * 100;
fprintf("能耗考虑再生制动—— 恒速：%f kWh | nmpc：%f kWh\n",energy_cruise_plus_eb, energy_opt_plus_eb);
fprintf("考虑%f再生制动利用的节能率：%f\n",recycle_ratio, ee_ratio3);

%计算温度主动抑制效果
cr_Ts_avg = sum(cr_T(:,1))/length(cr_T(:,1)); %恒速-定子-平均温度
cr_Ts_max = max(cr_T(:,1));
cr_Tr_avg = sum(cr_T(:,2))/length(cr_T(:,2)); %恒速-转子-平均温度
cr_Tr_max = max(cr_T(:,2));
opt_Ts_avg = sum(T_stator)/length(T_stator); %nmpc-定子-平均温度
opt_Ts_max = max(T_stator);
opt_Tr_avg = sum(T_rotor)/length(T_rotor); %nmpc-转子-平均温度
opt_Tr_max = max(T_rotor);
fprintf("恒速——转子最高温度：%f | 定子最高温度：%f\n", cr_Tr_max, cr_Ts_max);
fprintf("nmpc——转子最高温度：%f | 定子最高温度：%f\n", opt_Tr_max, opt_Ts_max);
fprintf("定子——平均温度降低：%f | 最高温度降低：%f\n", cr_Ts_avg-opt_Ts_avg, cr_Ts_max-opt_Ts_max);
fprintf("转子——平均温度降低：%f | 最高温度降低：%f\n", cr_Tr_avg-opt_Tr_avg, cr_Tr_max-opt_Tr_max);

%结果存到矩阵方便复制
out_mat = [QsInfo.pos_start*1000 QsInfo.pos_start*1000 0;
            cr_s(end)  mpc_s(end) x_end_err;
            tcr_stop t_stop t_end_err;
            energy_cruise_kwh energy_opt_kwh ee_ratio;
            pmotor_in_cruise pmotor_in_nmpc ee_ratio2;
            ec1 eo1 ec1-eo1;
            energy_cruise_plus_eb energy_opt_plus_eb ee_ratio3;
            cr_Ts_max opt_Ts_max cr_Ts_max-opt_Ts_max;
            cr_Ts_avg opt_Ts_avg cr_Ts_avg-opt_Ts_avg;
            cr_Tr_max opt_Tr_max cr_Tr_max-opt_Tr_max;
            cr_Tr_avg opt_Tr_avg cr_Tr_avg-opt_Tr_avg];
out_mat = real(out_mat);

%% 画图
%值采样
x_km = mpc_s./1000;
v_kmh = mpc_vms*3.6;
t_s = tc;
f_kN = [mpc_f; mpc_f(end)];

%计算坡度
init_h = LineInfo.init_height;
h_(1) = init_h;
for j = 1:length(LineInfo.gradient)
    h_(j+1) = h_(j) + (LineInfo.gradient(j,2)-LineInfo.gradient(j,1))*LineInfo.gradient(j,3)/1000;
end
h_ = h_';
%处理限速
start_points = LineInfo.speedlimit(:, 1);
end_points = LineInfo.speedlimit(:, 2);
speed_values = LineInfo.speedlimit(:, 3);
vl_x = [];
vl_y = [];
for i2 = 1:size(LineInfo.speedlimit, 1)
    % 每段限速数据
    vl_x = [vl_x; start_points(i2); end_points(i2)];
    vl_y = [vl_y; speed_values(i2); speed_values(i2)];
end
vl_x = vl_x./1000; %m->km
%判断上下行
if QsInfo.pos_start > QsInfo.pos_end
    upordown = -1;
else
    upordown = 1;
end

figure(1)
set(gcf,'Position',[100,50,800,900]);
[ha,pos] = tight_subplot(4, 1,[.005 .05],[.05 .03],[.08 .07]); % 缩小子图间距
axes(ha(1));
hl3 = plot(cr_s./1000,cr_v_kmh,'LineWidth',2,'Color',[0.8500 0.3250 0.0980]);
hold on;
set(hl3, 'DisplayName', "恒速优化")
hl2 = plot(x_km,v_kmh,'LineWidth',2,'Color',[0 0.4470 0.7410]);
set(hl2, 'DisplayName', "实际速度")
% hl3 = plot(new_mat(:,2),new_mat(:,8),'LineStyle',':','LineWidth',2.5,'Color',[0.8500 0.3250 0.0980]);
% set(hl3, 'DisplayName', "参考速度")
hl4 = plot(vl_x,vl_y,'LineStyle',':','LineWidth',1.5,'Color',	[1 0 1]);
set(hl4, 'DisplayName', "限速")
% xlim([min(new_mat(:,2))-0.2 max(new_mat(:,2))+0.2]);
% ylim([0 max(v_kmh)+20]);
xlabel("位置（km）");
ylabel("速度（km/h）");
if upordown == -1
    set(gca, 'XDir', 'reverse');
end
lgd = legend('show','Location','south');
axis([min(x_km)-0.2 max(x_km)+0.2 0 max(LineInfo.speedlimit(:,3))+20]);
% set(gca, 'XTickMode', 'auto', 'XTickLabelMode', 'auto');
% grid on;

% set(lgd, 'Position', [0, 0, 50, 60]); % 自定义位置
% title("空间域跟踪效果图");
axes(ha(2));
hl1 = area([LineInfo.gradient(:,1);LineInfo.gradient(end,2)]./1000,h_,'FaceColor',[0.7 0.7 0.7],'EdgeColor', [0.6 0.6 0.6],"FaceAlpha",0.35);
set(hl1, 'DisplayName', '坡度');
grid on;
legend("坡度")
xlim([min(x_km)-0.2 max(x_km)+0.2]);
ylim([min(h_)-50 max(h_)*1.3]);
ylabel("相对海拔（m）");
if upordown == -1
    set(gca, 'XDir', 'reverse');
end

axes(ha(3));
plot(cr_s./1000,cr_f,'Color',[0.8500 0.3250 0.0980]);
hold on;
plot(x_km,f_kN,'LineWidth',2,'Color',[0 0.4470 0.7410]);
plot(x_km,[mpc_fe;mpc_fe(end)],'LineStyle','--');
plot(x_km,-[mpc_fm;mpc_fm(end)],'LineStyle','-.');
% plot(new_mat(:,2),new_mat(:,5),'LineStyle',':','LineWidth',2.5);
% plot(new_mat(:,2),new_mat(:,7),'LineStyle','--','LineWidth',1.5);
plot([-1 1e6],[0 0],'LineStyle','--','Color',[0 0 0])
xlabel("位置（km）");
ylabel("力（kN）");
axis([min(x_km)-0.2 max(x_km)+0.2 min(f_kN)-50 max(f_kN)+50]);
if upordown == -1
    set(gca, 'XDir', 'reverse');
end
% grid on;
legend("恒速优化力","实际力","电机力","空气制动力","Location","south");

axes(ha(4))
plot(x_km,T_stator,'LineWidth',2,'Color',[0 0.4470 0.7410]);
hold on;
plot(x_km,T_rotor,'LineWidth',2,'Color',[0.8500 0.3250 0.0980]);
plot(cr_s./1000,cr_T(:,1),'LineStyle','--','Color',[0 0.4470 0.7410]);
plot(cr_s./1000,cr_T(:,2),'LineStyle','--','Color',[0.8500 0.3250 0.0980]);
plot(x_km,mpc_T_env,'LineWidth',1.5,'LineStyle',':');
grid on;
axis([min(x_km)-0.2 max(x_km)+0.2 min(mpc_T_env)-20 200]);
xlabel("位置（km）");
ylabel("温度（℃）");
legend("定子温度","转子温度","恒速定子温度","恒速转子温度","沿路环境温度","Location","northwest");

%% 画时域图
figure(2)
set(gcf,'Position',[100,50,800,700]);
[ha,pos] = tight_subplot(3, 1,[.005 .05],[.07 .03],[.08 .07]); % 缩小子图间距
axes(ha(1));
plot(state(:,3),state(:,2),'LineWidth',2,'Color',[0.8500 0.3250 0.0980]);
hold on;
plot(tc,v_kmh(1:end-1),'LineWidth',2,'Color',[0 0.4470 0.7410]);
xlim([0 QsInfo.set_time+50]);
ylabel("速度（km/h）");

axes(ha(2))
plot(tc,mpc_fe,'LineWidth',2,'Color',[0 0.4470 0.7410]);
hold on;
plot([0 1e6],[0 0],'LineStyle','--','Color',[0 0 0])
xlim([0 QsInfo.set_time+50]);
ylabel("力（kN）");

axes(ha(3))
plot(tc,T_stator(1:end-1),'LineWidth',2,'Color',[0 0.4470 0.7410])
hold on;
plot(tc,T_rotor(1:end-1),'LineWidth',2,'Color',[0.8500 0.3250 0.0980]);
plot(tcr_,cr_T(:,1),'LineStyle','--','Color',[0 0.4470 0.7410]);
plot(tcr_,cr_T(:,2),'LineStyle','--','Color',[0.8500 0.3250 0.0980]);
grid on;
xlim([0 QsInfo.set_time+60]);
xlabel("时间（s）");
ylabel("温度（℃）");
legend("定子温度","转子温度","恒速定子温度","恒速转子温度","Location","northwest");
%% 画每次优化的温度结果
figure(77)
subplot(2,1,1)
for i = 1:numel(opt_proc)
    proc_res = opt_proc{1,i};
    if isempty(proc_res)
        continue;
    end
    h2 = plot(proc_res(:,1),proc_res(:,5));
    hold on;
    plot(proc_res(1,1),proc_res(1,5),'Marker','|','Color','r','LineWidth',3,'HandleVisibility', 'off');
    set(h2, 'DisplayName', "第"+num2str(i)+"次优化温度线");
end
h2 = plot(tc,T_rotor(1:end-1),'LineWidth',2,'LineStyle','--');
set(h2, 'DisplayName', "实际温度增长线");
lgd = legend('show','Location','northwest');
xlabel("时间 s");
ylabel("转子温度 ")

subplot(2,1,2)
for i = 1:numel(opt_proc)
    proc_res = opt_proc{1,i};
    if isempty(proc_res)
        continue;
    end
    h2 = plot(proc_res(:,1),proc_res(:,4));
    hold on;
    plot(proc_res(1,1),proc_res(1,4),'Marker','|','Color','r','LineWidth',3,'HandleVisibility', 'off');
    set(h2, 'DisplayName', "第"+num2str(i)+"次优化温度线");
end
h2 = plot(tc,T_stator(1:end-1),'LineWidth',2,'LineStyle','--');
set(h2, 'DisplayName', "实际温度增长线");
lgd = legend('show','Location','northwest');
xlabel("时间 s");
ylabel("定子温度 ");
